#include "data_structures.hpp"
#include <cmath>
#include <algorithm>
#include <numeric>
#include <iostream>
#include <sstream>
#include <iomanip>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace EddyDetection {

namespace {

    double bilinearInterpolation(const Eigen::MatrixXd& field,
                                 const Eigen::VectorXd& x_coords,
                                 const Eigen::VectorXd& y_coords,
                                 double x, double y) {

        int i = 0, j = 0;
        for (i = 0; i < x_coords.size() - 1; ++i) {
            if (x >= x_coords[i] && x <= x_coords[i + 1]) break;
        }
        for (j = 0; j < y_coords.size() - 1; ++j) {
            if (y >= y_coords[j] && y <= y_coords[j + 1]) break;
        }
        
        if (i >= x_coords.size() - 1) i = x_coords.size() - 2;
        if (j >= y_coords.size() - 1) j = y_coords.size() - 2;
        if (i < 0) i = 0;
        if (j < 0) j = 0;
        
        double x1 = x_coords[i], x2 = x_coords[i + 1];
        double y1 = y_coords[j], y2 = y_coords[j + 1];

        double Q11 = field(i, j);
        double Q12 = field(i, j + 1);  
        double Q21 = field(i + 1, j);
        double Q22 = field(i + 1, j + 1);

        double wx = (x2 - x1) != 0 ? (x - x1) / (x2 - x1) : 0.0;
        double wy = (y2 - y1) != 0 ? (y - y1) / (y2 - y1) : 0.0;
        
        return Q11 * (1 - wx) * (1 - wy) + 
               Q21 * wx * (1 - wy) + 
               Q12 * (1 - wx) * wy + 
               Q22 * wx * wy;
    }

    double calculateSphericalPolygonArea(const std::vector<Eigen::Vector2d>& contour) {
        if (contour.size() < 3) return 0.0;
        
        const double R_EARTH = 6371.0;
        double area = 0.0;

        double sum = 0.0;
        int n = contour.size();
        
        for (int i = 0; i < n; ++i) {
            int j = (i + 1) % n;
            double lat1 = contour[i][0] * M_PI / 180.0;
            double lon1 = contour[i][1] * M_PI / 180.0;
            double lat2 = contour[j][0] * M_PI / 180.0;
            double lon2 = contour[j][1] * M_PI / 180.0;
            
            double dlon = lon2 - lon1;
            
            sum += dlon * (2.0 + std::sin(lat1) + std::sin(lat2));
        }
        
        area = std::abs(sum) * R_EARTH * R_EARTH / 2.0;
        return area;
    }

    double calculatePlanarPolygonArea(const std::vector<Eigen::Vector2d>& contour) {
        if (contour.size() < 3) return 0.0;

        double center_lat = 0.0, center_lon = 0.0;
        for (const auto& point : contour) {
            center_lat += point[0];
            center_lon += point[1];
        }
        center_lat /= contour.size();
        center_lon /= contour.size();

        double mean_lat_rad = center_lat * M_PI / 180.0;
        double km_per_deg_lon = 111.32 * std::cos(mean_lat_rad);
        double km_per_deg_lat = 111.32;

        double area = 0.0;
        int n = contour.size();
        
        for (int i = 0; i < n; ++i) {
            int j = (i + 1) % n;
            double x1 = (contour[i][1] - center_lon) * km_per_deg_lon;
            double y1 = (contour[i][0] - center_lat) * km_per_deg_lat;
            double x2 = (contour[j][1] - center_lon) * km_per_deg_lon;
            double y2 = (contour[j][0] - center_lat) * km_per_deg_lat;
            
            area += x1 * y2 - x2 * y1;
        }
        
        return std::abs(area) / 2.0;
    }
}

double EddyAnalyzer::calculateArea(const std::vector<Eigen::Vector2d>& contour) {
    if (contour.size() < 3) return 0.0;

    // Choose a calculation method based on the size of the contour
    double max_extent = 0.0;
    for (size_t i = 0; i < contour.size(); ++i) {
        for (size_t j = i + 1; j < contour.size(); ++j) {
            double dx = contour[i][0] - contour[j][0];
            double dy = contour[i][1] - contour[j][1];
            double dist = std::sqrt(dx*dx + dy*dy);
            max_extent = std::max(max_extent, dist);
        }
    }

    // If max_extent > 5 degrees, use spherical geometry; Otherwise, use a planar approximation
    if (max_extent > 5.0) {
        return calculateSphericalPolygonArea(contour);
    } else {
        return calculatePlanarPolygonArea(contour);
    }
}

double calculateDistance(const double& latMax, const double& latMin) {
    // Calculate the distance between two latitudes in kilometers
    // Using spherical geometry for accurate distance calculation

    // Convert latitudes to radians
    double lat1_rad = latMax * M_PI / 180.0;
    double lat2_rad = latMin * M_PI / 180.0;

    // Earth's radius in kilometers
    const double R_EARTH = 6371.0;

    // Calculate the difference in latitude
    double dlat = lat1_rad - lat2_rad;

    // For latitude-only distance calculation (same longitude)
    // The distance is simply the arc length along a meridian
    double distance = R_EARTH * std::abs(dlat);

    return distance;
}

double EddyAnalyzer::calculateRadius(const std::vector<Eigen::Vector2d>& contour) {
    if (contour.size() < 2) return 0.0;

    // Maximum and minimum latitude values
    double latMax = -91;
    double latMin = 91;
    for (size_t i = 0; i < contour.size(); ++i) {
        double lat = contour[i][0];
        latMax = std::max(latMax, lat);
        latMin = std::min(latMin, lat);
    }

    // Calculate latitude distance
    double distance = calculateDistance(latMax, latMin);

    return distance / 2;
}

Eigen::Vector2d EddyAnalyzer::calculateCenter(const std::vector<Eigen::Vector2d>& contour) {
    if (contour.empty()) return Eigen::Vector2d(0.0, 0.0);

    // Calculate the center of mass
    double sum_x = 0.0, sum_y = 0.0;
    for (const auto& point : contour) {
        sum_x += point[0];
        sum_y += point[1];
    }
    
    return Eigen::Vector2d(sum_x / contour.size(), sum_y / contour.size());
}

double EddyAnalyzer::calculateMeanProperty(const std::vector<Eigen::Vector2d>& contour,
                                           const Eigen::MatrixXd& field,
                                           const Eigen::VectorXd& lat,
                                           const Eigen::VectorXd& lon) {
    if (contour.empty()) return 0.0;
    
    std::vector<double> values;
    values.reserve(contour.size());
    
    for (const auto& point : contour) {
        double value = bilinearInterpolation(field, lat, lon, point[0], point[1]);
        if (std::isfinite(value)) {
            values.push_back(value);
        }
    }
    
    if (values.empty()) return 0.0;
    
    return std::accumulate(values.begin(), values.end(), 0.0) / values.size();
}

EddyContour EddyAnalyzer::analyzeEddy(const std::vector<Eigen::Vector2d>& contour,
                                       const Eigen::MatrixXd& vorticity_field,
                                       const Eigen::MatrixXd& temp_field,
                                       const Eigen::VectorXd& lat,
                                       const Eigen::VectorXd& lon,
                                       double min_area_km2) {
    EddyContour eddy;
    eddy.boundary_points = contour;

    // Calculate basic geometric properties
    eddy.area_km2 = calculateArea(contour);
    eddy.radius = calculateRadius(contour);
    eddy.center = calculateCenter(contour);

    // If the area is too small, return an invalid eddy
    if (eddy.area_km2 < min_area_km2) {
        eddy.area_km2 = 0.0;
        return eddy;
    }

    // Calculate the average vorticity
    eddy.mean_vorticity = calculateMeanProperty(contour, vorticity_field, lat, lon);

    // Determine the average omega of eddy
    eddy.mean_omega = fabs(eddy.mean_vorticity / 2);

    //Determine the type of eddy
    //eddy.type = (eddy.mean_vorticity > 0) ? "Cyclonic" : "Anticyclonic";
    eddy.type = (eddy.mean_vorticity > 0) ? EddyContourDataCyclone::cyclone : EddyContourDataCyclone::anticyclone;

    // Calculate temperature anomalies
    double temp_inside = calculateMeanProperty(contour, temp_field, lat, lon);

    // Calculate boundary temperature
    std::vector<double> boundary_temps;
    for (const auto& point : contour) {
        double temp = bilinearInterpolation(temp_field, lat, lon, point[0], point[1]);
        if (std::isfinite(temp)) {
            boundary_temps.push_back(temp);
        }
    }
    
    double temp_boundary = 0.0;
    if (!boundary_temps.empty()) {
        temp_boundary = std::accumulate(boundary_temps.begin(), boundary_temps.end(), 0.0) / boundary_temps.size();
    }
    
    eddy.temp_anomaly = temp_inside - temp_boundary;

    // Determine Warm/Cold type
    if (eddy.temp_anomaly > 0) {
        eddy.thermal_type = EddyContourDataThermal::WarmCore;
    } else {
        eddy.thermal_type = EddyContourDataThermal::Cold_core;
    }
    
    return eddy;
}

std::vector<EddyContour> EddyAnalyzer::filterAndSortEddies(
    std::vector<EddyContour>& eddies, double min_area_km2, const VelocityField& velocity_field) {

    //// Filter out eddys where the u-velocity at max lat equals the u-velocity at min lat
    //auto it = std::remove_if(eddies.begin(), eddies.end(),
    //    [&velocity_field](const EddyContour& eddy) {
    //        if (eddy.boundary_points.empty()) return true;
    //        
    //        // Find the point with maximum latitude
    //        auto max_lat_it = std::max_element(eddy.boundary_points.begin(), eddy.boundary_points.end(),
    //            [](const Eigen::Vector2d& a, const Eigen::Vector2d& b) {
    //                return a[0] < b[0]; // a[0] is latitude
    //            });
    //        
    //        // Find the point with minimum latitude
    //        auto min_lat_it = std::min_element(eddy.boundary_points.begin(), eddy.boundary_points.end(),
    //            [](const Eigen::Vector2d& a, const Eigen::Vector2d& b) {
    //                return a[0] < b[0]; // a[0] is latitude
    //            });
    //        
    //        if (max_lat_it == eddy.boundary_points.end() || min_lat_it == eddy.boundary_points.end()) {
    //            return true;
    //        }
    //        
    //        // Get u-velocity values at max and min lat points using bilinear interpolation
    //        double u_max_lat = bilinearInterpolation(velocity_field.u(), velocity_field.lat, velocity_field.lon, 
    //                                               (*max_lat_it)[0], (*max_lat_it)[1]);
    //        double u_min_lat = bilinearInterpolation(velocity_field.u(), velocity_field.lat, velocity_field.lon, 
    //                                               (*min_lat_it)[0], (*min_lat_it)[1]);
    //        
    //        // Remove eddy if u-velocities are in the same direction (both positive or both negative)
    //        // If product > 0, they are in the same direction; if product < 0, they are in opposite directions
    //        return (u_max_lat * u_min_lat) > 0;
    //    });
    //eddies.erase(it, eddies.end());

    //// Filter out eddys where the v-velocity at max lon equals the v-velocity at min lon
    //auto it1 = std::remove_if(eddies.begin(), eddies.end(),
    //    [&velocity_field](const EddyContour& eddy) {
    //        if (eddy.boundary_points.empty()) return true;

    //        // Find the point with maximum longitude
    //        auto max_lon_it = std::max_element(eddy.boundary_points.begin(), eddy.boundary_points.end(),
    //            [](const Eigen::Vector2d& a, const Eigen::Vector2d& b) {
    //                return a[1] < b[1]; // a[1] is longitude
    //            });

    //        // Find the point with minimum longitude
    //        auto min_lon_it = std::min_element(eddy.boundary_points.begin(), eddy.boundary_points.end(),
    //            [](const Eigen::Vector2d& a, const Eigen::Vector2d& b) {
    //                return a[1] < b[1]; // a[1] is longitude
    //            });

    //        if (max_lon_it == eddy.boundary_points.end() || min_lon_it == eddy.boundary_points.end()) {
    //            return true;
    //        }

    //        // Get v-velocity values at max and min lon points using bilinear interpolation
    //        double v_max_lon = bilinearInterpolation(velocity_field.v(), velocity_field.lat, velocity_field.lon, 
    //                                               (*max_lon_it)[0], (*max_lon_it)[1]);
    //        double v_min_lon = bilinearInterpolation(velocity_field.v(), velocity_field.lat, velocity_field.lon, 
    //                                               (*min_lon_it)[0], (*min_lon_it)[1]);

    //        // Remove eddy if v-velocities are in the same direction (both positive or both negative)
    //        // If product > 0, they are in the same direction; if product < 0, they are in opposite directions
    //        return (v_max_lon * v_min_lon) > 0;
    //    });
    //eddies.erase(it1, eddies.end());
    
    // Filter out eddys with too small an area
    auto it2 = std::remove_if(eddies.begin(), eddies.end(),
        [min_area_km2](const EddyContour& eddy) {
            return eddy.area_km2 < min_area_km2;
        });
    eddies.erase(it2, eddies.end());

    // Sort area from large to small
    std::sort(eddies.begin(), eddies.end(),
        [](const EddyContour& a, const EddyContour& b) {
            return a.area_km2 > b.area_km2;
        });

    // Assign ID
    for (size_t i = 0; i < eddies.size(); ++i) {
        std::stringstream ss;
        ss << "EDDY-" << std::setfill('0') << std::setw(3) << (i + 1);
        eddies[i].id = ss.str();
    }
    
    return eddies;
}

} // namespace EddyDetection 